#light

namespace CGraph.Math.Types


open CGraph.CGMath
open CGraph.Math.Shoemake

type Vec3 =
    val m_data : double array
    new() = {
        m_data = [| 0.; 0.; 0. |]
    }
    new( x, y, z) = {
        m_data = [| x;y;z |]
    }
    member this.Data = this.m_data
    
    member this.Item 
        with get(idx) = this.m_data.[idx]
        and set(idx,value) = this.m_data.[idx] <- value
    
    static member ( + ) ((this:Vec3),(other:Vec3)) =
        Vec3( this.m_data.[0] + other.m_data.[0],
            this.m_data.[1] + other.m_data.[1],
            this.m_data.[2] + other.m_data.[2] )
            
    static member ( - ) ((this:Vec3),(other:Vec3)) =
        Vec3( this.m_data.[0] - other.m_data.[0],
            this.m_data.[1] - other.m_data.[1],
            this.m_data.[2] - other.m_data.[2] )
            
    static member ( * ) ((this:Vec3),(other:Vec3)) =
        let inline aget idx = this.m_data.[idx]
        let inline bget idx = other.m_data.[idx]
        VDot3In aget bget
        
    /// cross
    member this.Cross ((other:Vec3)) =
        let inline aget idx = this.m_data.[idx]
        let inline bget idx = other.m_data.[idx]
        let retval = Vec3()
        let inline set idx data = retval.m_data.[idx] <- data
        VCross3In aget bget set
        retval
        
    member this.Normalize() =
        let inline aget idx = this.m_data.[idx]
        let retval = Vec3()
        let inline set idx data = retval.m_data.[idx] <- data
        let len = VNorm3In aget set
        retval,len
        
    member this.Length =
        let inline aget idx = this.m_data.[idx]
        VLen3 aget
        
type Vec4 =
    val m_data : double array
    new() = {
        m_data = [| 0.; 0.; 0.; 0. |]
    }
    new( x, y, z, w) = {
        m_data = [| x;y;z;w |]
    }
    member this.Data = this.m_data
    
    member this.Item 
        with get(idx) = this.m_data.[idx]
        and set(idx,value) = this.m_data.[idx] <- value
    
    static member ( + ) ((this:Vec4),(other:Vec4)) =
        Vec4( this.m_data.[0] + other.m_data.[0],
            this.m_data.[1] + other.m_data.[1],
            this.m_data.[2] + other.m_data.[2],
            this.m_data.[3] + other.m_data.[3] )
            
    static member ( - ) ((this:Vec4),(other:Vec4)) =
        Vec4( this.m_data.[0] - other.m_data.[0],
            this.m_data.[1] - other.m_data.[1],
            this.m_data.[2] - other.m_data.[2],
            this.m_data.[3] - other.m_data.[3] )
            
    static member ( * ) ((this:Vec4),(other:Vec4)) =
        let inline aget idx = this.m_data.[idx]
        let inline bget idx = other.m_data.[idx]
        VDot4In aget bget
        
    member this.Normalize() =
        let inline aget idx = this.m_data.[idx]
        let retval = Vec4()
        let inline set idx data = retval.m_data.[idx] <- data
        let len = VNorm4In aget set
        retval,len
        
    member this.Length =
        let inline aget idx = this.m_data.[idx]
        VLen4 aget
        
type Quat =
    inherit Vec4
    new() = {}
    new(x,y,z,w) = {
        inherit Vec4(x,y,z,w)
    }
    //This assumes a normalized axis already
    new(axis:Vec3,angle:double) as this = 
        {
        }
        then
            let inline set idx value = this.m_data.[idx] <- value
            let adata = axis.Data
            let inline aget idx = adata.[idx]
            QuatFromAxisAngleIn aget angle set
    
    member this.Conj() =
        let inline get idx = this.m_data.[idx]
        let retval = Quat()
        let rdata = retval.Data
        let inline rset idx value = rdata.[idx] <- value
        QuatConjIn get rset
        
    static member ( * ) ((this:Quat),(other:Quat)) =
        let inline aget idx = this.m_data.[idx]
        let inline bget idx = other.m_data.[idx]
        let retval = Quat()
        let rdata = retval.Data
        let inline rset idx value = rdata.[idx] <- value
        QuatMulIn aget bget rset
        retval
        
        
type Matrix44 =
    val m_data: double array array
    new() = {
        m_data = [|[|1.;0.;0.;0.|]
                   [|0.;1.;0.;0.|]
                   [|0.;0.;1.;0.|]
                   [|0.;0.;0.;1.|]|]
    }
    new( m00, m01, m02, m03,
         m04, m05, m06, m07,
         m08, m09, m10, m11,
         m12, m13, m14, m15 ) = {
         m_data = [|[|m00; m01; m02; m03|]
                    [|m04; m05; m06; m07|]
                    [|m08; m09; m10; m11|]
                    [|m12; m13; m14; m15|]|]
    }
    new( quat:Quat ) as this = 
        {
            m_data = [|[|1.;0.;0.;0.|]
                       [|0.;1.;0.;0.|]
                       [|0.;0.;1.;0.|]
                       [|0.;0.;0.;1.|]|]
        }
        then
            let inline get idx = quat.[idx]
            let inline set col row value = this.m_data.[row].[col] <- value
            QuatToMatrixIn get set
            
    member this.Data = this.m_data
    member this.Item
        with get(row,column) = this.m_data.[row].[column]
        and set(row,column,value) = this.m_data.[row].[column] <- value
        
    member this.Copy (other:Matrix44) =
        for i in 0..3 do
            for j in 0..3 do
                this.m_data.[i].[j] <- other.m_data.[i].[j]
            
    [<OverloadID("MultMM")>]
    static member ( * ) ((this:Matrix44),(other:Matrix44)) =
        let inline aget col row = this.m_data.[row].[col]
        let inline bget col row = other.m_data.[row].[col]
        let retval = Matrix44()
        let inline set col row value = retval.m_data.[row].[col] <- value
        MMult4In aget bget set
        retval
    
    [<OverloadID("MultMV")>]
    static member ( * ) ((other:Vec4),(this:Matrix44)) =
        let inline aget col row = this.m_data.[row].[col]
        let xdata = other.Data
        let inline xget idx = xdata.[idx]
        let retval = Vec4()
        let rdata = retval.Data
        let inline rset idx value = rdata.[idx] <- value
        VMult4In aget xget rset
        retval
        
    member this.SwapRows( row1, row2 ) =
        let temp = this.m_data.[row1]
        this.m_data.[row1] <- this.m_data.[row2]
        this.m_data.[row2] <- temp
    
    member this.ForwardElimination( (other:Matrix44) ) =
        let inline mget col row =
            if row > 3
                then
                    other.m_data.[col].[row-4]
                else
                    this.m_data.[col].[row]
        let inline mset col row value =
            if row > 3
                then
                    other.m_data.[col].[row-4] <- value
                else
                    this.m_data.[col].[row] <- value
                    
        let inline swap row1 row2 =
            this.SwapRows( row1,row2 )
            other.SwapRows(row1,row2 )
        ForwardElimination 4 8 mget mset swap
        
    member this.BackSubstitution( (other:Matrix44) ) =
        let inline mget col row =
            if row > 3
                then
                    other.m_data.[col].[row-4]
                else
                    this.m_data.[col].[row]
        let inline mset col row value =
            if row > 3
                then
                    other.m_data.[col].[row-4] <- value
                else
                    this.m_data.[col].[row] <- value
                 
        BackSubstitution 4 8 mget mset
        
    member this.Invert() =
        let other = Matrix44()
        let inline mget col row =
            if row > 3
                then
                    other.m_data.[col].[row-4]
                else
                    this.m_data.[col].[row]
        let inline mset col row value =
            if row > 3
                then
                    other.m_data.[col].[row-4] <- value
                else
                    this.m_data.[col].[row] <- value
                    
        let inline swap row1 row2 =
            this.SwapRows( row1,row2 )
            other.SwapRows(row1,row2 )
            
        let retval = ForwardEliminationIn 4 8 mget mset swap
        if retval 
            then 
                (BackSubstitutionIn 4 8 mget mset ) |> ignore
                this.Copy other
        retval
    
    member this.Transpose() =
        let inline aget col row = this.m_data.[row].[col]
        let retval = Matrix44()
        let inline set row col value = retval.Data.[row].[col] <- value
        for i in 0..3 do
            for j in 0..3 do
                set i j (aget i j)
        
        
    member this.Translate (vec:Vec3) =
        let temp1 = Matrix44()
        temp1.m_data.[3].[0] <- vec.[0]
        temp1.m_data.[3].[1] <- vec.[1]
        temp1.m_data.[3].[2] <- vec.[2]
        let temp2 = this * temp1
        this.Copy temp2
        
    member this.Scale (vec:Vec3) =
        let temp1 = Matrix44()
        temp1.m_data.[0].[0] <- vec.[0]
        temp1.m_data.[1].[1] <- vec.[1]
        temp1.m_data.[2].[2] <- vec.[2]
        let temp2 = this * temp1
        this.Copy temp2
        
    member this.Rotate (quat:Quat) =
        let temp1 = Matrix44(quat)
        let temp2 = this * temp1
        this.Copy temp2
        
    /// Not a cheap operation...
    /// trans,rot,stretchRot,stretch,detSign
    member this.Decompose() =
        let s = single
        let inline tos row col = s(this.m_data.[col].[row])
        let inline toArray idx = [|(tos idx 0);(tos idx 1);(tos idx 2);(tos idx 3)|]
        let smat = [|(tos 0 0);(tos 0 1);(tos 0 2);(tos 0 3);
                      (tos 1 0);(tos 1 1);(tos 1 2);(tos 1 3);
                      (tos 2 0);(tos 2 1);(tos 2 2);(tos 2 3);
                      (tos 3 0);(tos 3 1);(tos 3 2);(tos 3 3)|]
        let decomp = DecompAffine smat
        
        let inline v idx = double(decomp.Translation.[idx])
        let trans = Vec4((v 0),(v 1),(v 2),(v 3))
        
        let inline v idx = double(decomp.Rotation.[idx])
        let rot = Quat((v 0),(v 1),(v 2),(v 3))
        
        let inline v idx = double(decomp.StretchRotation.[idx])
        let stretchRot = Quat((v 0),(v 1),(v 2),(v 3))
        
        let inline v idx = double(decomp.StretchFactors.[idx])
        let stretch = Vec4((v 0),(v 1),(v 2),(v 3))
        
        let detSign = double(decomp.DeterminantSign)
        trans,rot,stretchRot,stretch,detSign
        